function [cost] = PlotCurrentFunction(settings,weather,aircraft,target,time,alt,fig)
%*=+--+=#=+--      UAS Severe Weather Simulation Softeware        --+=#=+--+=#*%
%          Copyright (C) 2013 Regents of the University of Colorado.           %
%                             All Rights Reserved.                             %
%                                                                              %
%    This program is free software: you can redistribute it and/or modify      %
%    it under the terms of the GNU General Public License version 2 as         %
%    published by the Free Software Foundation.                                %
%                                                                              %
%    This program is distributed in the hope that it will be useful,           %
%    but WITHOUT ANY WARRANTY; without even the implied warranty of            %
%    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             %
%    GNU General Public License for more details.                              %
%                                                                              %
%    You should have received a copy of the GNU General Public License         %
%    along with this program.  If not, see <http://www.gnu.org/licenses/>.     %
%                                                                              %
%           Jack Elston                                                        %
%           elstonj@colorado.edu                                               %
%                                                                              %
%*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*%

k=floor((alt-weather.z0)/weather.zs) +1;

target_x = floor((target.x-weather.x0)/weather.xs) +1;
target_y = floor((target.y-weather.y0)/weather.ys) +1;

cost(:,:,k) = ones(weather.xnum, weather.ynum)*Inf;
cost(target_x,target_y,k) = 0;

figure(fig); clf;
PlotCircle(fig,target.x,target.y,0.2); hold on;
plot(target.x,target.y,'*g'); %50m search radius
title(strcat('Wavefront Planner time:',num2str(time),'s'));
axis([weather.x0-10, weather.xf+10, weather.y0-10, weather.yf+10]);
grid;

dist = max([weather.xnum, weather.ynum]);
dist = 20;

for i=1:dist

  m0 = target_x - i;
  if m0 < 1
    m0 = 1;
  end
  mf = target_x + i;
  if mf > weather.xnum
    mf = weather.xnum;
  end
  n0 = target_y - i;
  if n0 < 1
    n0 = 1;
  end
  nf = target_y + i;
  if nf > weather.ynum
    nf = weather.ynum;
  end

  n = nf;
  if(target_y + i <= weather.ynum)
    for m = m0:mf
      cost = CalcCost(m,n,target,weather,time,alt,cost);
    end
  end

  m = mf;
  if(target_x + i <= weather.xnum)
    if(target_y + i > weather.ynum)
      ntf = nf+1;
    else
      ntf = nf;
    end
    for n = ntf-1:-1:n0
      cost = CalcCost(m,n,target,weather,time,alt,cost);
    end
  end

  n=n0;
  if(target_y - i >= 1)
    if(target_x + i > weather.xnum)
      mtf = mf+1;
    else
      mtf = mf;
    end
    for m = mtf-1:-1:m0
      cost = CalcCost(m,n,target,weather,time,alt,cost);
    end
  end

  m=m0;
  if(target_x - i >= 1)
    if(target_y - i < 1)
      nt0 = 0;
    else
      nt0 = n0;
    end
    if(target_y + i > weather.ynum)
      ntf = nf+1;
    else
      ntf = nf;
    end
    for n = nt0+1:ntf-1
      cost = CalcCost(m,n,target,weather,time,alt,cost);
    end
  end

end

fig = 2;

x = weather.x;
y = weather.y;

%[py,px]=gradient(cost,weather.xs,weather.ys);
%px = px(1:2:weather.xnum,1:2:weather.ynum);
%py = py(1:2:weather.xnum,1:2:weather.ynum);

figure(fig); hold off;
%[cs,ch]=contourf(x(:,:,k),y(:,:,k),cost(:,:,k)',[0:0.4:4]);
[cs,ch]=contourf(x(:,:,k),y(:,:,k),cost(:,:,k)',[0:0.5:8]);
%surf(x(:,:,k),y(:,:,k),cost(:,:,k)');
hold on;
%h1=quiver(x(1:2:weather.xnum),y(1:2:weather.ynum),px,py);
%set(h1, 'Color',[1 1 1]);
PlotCircle(fig,target.x,target.y,1);
plot(target.x,target.y,'*g');
title(strcat('Wavefront Planner time:',num2str(time),'s'));
axis([weather.x0, weather.xf, weather.y0, weather.yf]);
hold off;

%cost(:,:,1)
